  /**
  ****************************(C) COPYRIGHT 2019 DJI****************************
  * @file       chassis_behaviour.c/h
  * @brief      according to remote control, change the chassis behaviour.
  *             根据遥控器的值，决定底盘行为。
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. done
  *  V1.1.0     Nov-11-2019     RM              1. add some annotation
  *
  ****************************(C) COPYRIGHT 2019 DJI****************************
  */



#include "chassis_behaviour.h"
#include "cmsis_os.h"
#include "chassis_task.h"
#include "arm_math.h"
#include "main.h"
#include "gimbal_behaviour.h"
#include "detect_task.h"
#include "user_lib.h"
#include "robot.h" 

extern chassis_move_t chassis_move;

#define rc_deadband_limit(input, output, dealine)        \
    {                                                    \
        if ((input) > (dealine) || (input) < -(dealine)) \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else                                             \
        {                                                \
            (output) = 0;                                \
        }                                                \
    }



/**
  * @brief          根据遥控器通道值，计算纵向和横移速度
  *                 
  * @param[out]     vx_set: 纵向速度指针
  * @param[out]     vy_set: 横向速度指针
  * @param[out]     chassis_move_rc_to_vector: "chassis_move" 变量指针
  * @retval         none
  */
 static void chassis_rc_to_control_vector(fp32 *vx_set, fp32 *vy_set)
 {
     if (vx_set == NULL || vy_set == NULL)
     {
         return;
     }
    
     int16_t vx_channel, vy_channel;
     fp32 vx_set_channel, vy_set_channel;
     //死区限制，因为遥控器可能存在差异 摇杆在中间，其值不为0
     rc_deadband_limit(chassis_move.chassis_RC->rc.ch[CHASSIS_X_CHANNEL], vx_channel, CHASSIS_RC_DEADLINE);
     rc_deadband_limit(chassis_move.chassis_RC->rc.ch[CHASSIS_Y_CHANNEL], vy_channel, CHASSIS_RC_DEADLINE);

     vx_set_channel = vx_channel * CHASSIS_VX_RC_SEN * RC_X_DIR;
     vy_set_channel = vy_channel * CHASSIS_VY_RC_SEN * RC_Y_DIR;

     //键盘控制
     if (chassis_move.chassis_RC->key.v & CHASSIS_FRONT_KEY)
     {
         vx_set_channel = CHASSIS_SEN;
     }
     else if (chassis_move.chassis_RC->key.v & CHASSIS_BACK_KEY)
     {
         vx_set_channel = -CHASSIS_SEN;
     }

     if (chassis_move.chassis_RC->key.v & CHASSIS_LEFT_KEY)
     {
         vy_set_channel = CHASSIS_SEN;
     }
     else if (chassis_move.chassis_RC->key.v & CHASSIS_RIGHT_KEY)
     {
         vy_set_channel = -CHASSIS_SEN;
     }

     //一阶低通滤波代替斜波作为底盘速度输入
     first_order_filter_cali(&chassis_move.chassis_cmd_slow_set_vx, vx_set_channel);
     first_order_filter_cali(&chassis_move.chassis_cmd_slow_set_vy, vy_set_channel);
     //停止信号，不需要缓慢加速，直接减速到零
     if (vx_set_channel < CHASSIS_RC_DEADLINE * CHASSIS_VX_RC_SEN && vx_set_channel > -CHASSIS_RC_DEADLINE * CHASSIS_VX_RC_SEN)
     {
         chassis_move.chassis_cmd_slow_set_vx.out = 0.0f;
     }

     if (vy_set_channel < CHASSIS_RC_DEADLINE * CHASSIS_VY_RC_SEN && vy_set_channel > -CHASSIS_RC_DEADLINE * CHASSIS_VY_RC_SEN)
     {
         chassis_move.chassis_cmd_slow_set_vy.out = 0.0f;
     }

     *vx_set = chassis_move.chassis_cmd_slow_set_vx.out;
     *vy_set = chassis_move.chassis_cmd_slow_set_vy.out;
 }

		
//进入模式和退出模式时单次动作需在return TRUE前写	
		
// ####################################################### DEBUG 调试模式 ####################################################### //
bool_t enterChassisDebugCondition() {
    #ifdef DEBUG_CHASSIS
    return TRUE;
    #endif

    #ifndef DEBUG_CHASSIS
    return FALSE;
    #endif
}

bool_t outChassisDebugCondition() {
        #ifdef DEBUG_CHASSIS
    return FALSE;
    #endif

    #ifndef DEBUG_CHASSIS
    return TRUE;
    #endif
}

void chassisDebugHandleFun(fp32* vx, fp32* vy, fp32* wz) {

		 if(IF_RC_SW1_DOWN&&IF_RC_SW2_DOWN){
			chassis_rc_to_control_vector(vx, vy);
			*wz = -CHASSIS_WZ_RC_SEN * chassis_move.chassis_RC->rc.ch[CHASSIS_WZ_CHANNEL];
		}
}
// ####################################################### zeroForce 无力模式 ####################################################### //
bool_t enterZeroForceCondition() {
    if(toe_is_error(DBUS_TOE) || (robotInf.modeStep < ROBOT_INIT_CHASSIS && robotInf.robotMode == ROBOT_INIT))
       return TRUE;
	
	return FALSE;
}

bool_t outZeroForceCondition() {
		if(!enterZeroForceCondition())
			return TRUE;
    return FALSE;
}

void zeroForceHandleFun(fp32* vx, fp32* vy, fp32* wz) {
     *vx = 0;
     *vy = 0;
     *wz = 0;
	

}
// ####################################################### CHASSIS_INIT 底盘初始化模式 ####################################################### //
bool_t enterChassisInitCondition() {
    if((*chassis_move.robotModeStep == ROBOT_INIT_CHASSIS && *chassis_move.robotMode == ROBOT_INIT))
        return TRUE;

    return FALSE;
}

bool_t outChassisInitCondition() {
    if(enterChassisInitCondition())
        return FALSE;

    return TRUE;
}

void chassisInitHandleFun(fp32* vx, fp32* vy, fp32* wz) {
	 *vx = 0;
	 *vy = 0;
	 *wz = 0;
	 *chassis_move.robotModeStep = ROBOT_INIT_FINISH;

}

// ####################################################### CHASSIS_ROTATE 小陀螺模式 ####################################################### //
bool_t enterChassisRotateCondition() {
    if(IF_KEY_PRESSED_R||(IF_RC_SW1_UP))	// 键盘小陀螺进入逻辑，需要和操作手协商，暂时未添加
			return TRUE;
		else
			return FALSE;
}

bool_t outChassisRotateCondition() {
    if((IF_KEY_PRESSED_CTRL&&IF_KEY_PRESSED_R)||!IF_RC_SW1_UP)	// 遥控器小陀螺退出逻辑
			return TRUE;
		else
			return FALSE;
}

void chassisRotateHandleFun(fp32* vx, fp32* vy, fp32* wz) {

		chassis_rc_to_control_vector(vx, vy);
    *wz =  chassis_move.relative_angle+CHASSIS_ROTATE_VZ;
//		*wz = loop_ramp_float(0, chassis_move.relative_angle_set, 180.0 / 1000 * CHASSIS_TASK_MS,\
//			chassis_move.minRelativeAngle, chassis_move.maxRelativeAngle);

}

// ####################################################### 坦克模式 ####################################################### //
bool_t enterChassisFollowSelfCondition()
{
    if(IF_KEY_PRESSED_F||(IF_RC_SW1_MID))	
			return TRUE;
		else
			return FALSE;
}

bool_t outChassisFollowSelfCondition()
{
    if((IF_KEY_PRESSED_CTRL&&IF_KEY_PRESSED_F)||!IF_RC_SW1_MID)	
			return TRUE;
		else
			return FALSE;
}

void chassisFollowSelfHandleFun(fp32* vx, fp32* vy, fp32* wz) {
	
     if (vx == NULL || vy == NULL || wz == NULL )
     {
         return;
     }

//     chassis_rc_to_control_vector(vx, vy);
//     *wz = rad_format(chassis_move.chassis_yaw_set - CHASSIS_ANGLE_Z_RC_SEN * chassis_move.chassis_RC->rc.ch[CHASSIS_WZ_CHANNEL]);
     chassis_rc_to_control_vector(vx, wz);
}

// ####################################################### 跟随模式（默认） ####################################################### //
bool_t enterChassisFollowCondition()
{
    return TRUE;
}

bool_t outChassisFollowCondition()
{
    return FALSE;
}

void chassisFollowHandleFun(fp32* vx, fp32* vy, fp32* wz) {
	
     if (vx == NULL || vy == NULL || wz == NULL)
     {
         return;
     }

     //遥控器的通道值以及键盘按键 得出 一般情况下的速度设定值
     chassis_rc_to_control_vector(vx, vy);
		 *wz = loop_ramp_float(0, chassis_move.relative_angle_set, 180.0 / 1000 * CHASSIS_TASK_MS,\
			chassis_move.minRelativeAngle, chassis_move.maxRelativeAngle);

}


/**
  * @brief          底盘无力的行为状态机下，底盘模式是raw，故而设定值会直接发送到can总线上故而将设定值都设置为0
  * @author         RM
  * @param[in]      vx_set前进的速度 设定值将直接发送到can总线上
  * @param[in]      vy_set左右的速度 设定值将直接发送到can总线上
  * @param[in]      wz_set旋转的速度 设定值将直接发送到can总线上
  * @param[in]      chassis_move_rc_to_vector底盘数据
  * @retval         返回空
  */

// static void chassis_zero_force_control(fp32 *vx_can_set, fp32 *vy_can_set, fp32 *wz_can_set, chassis_move_t *chassis_move_rc_to_vector)
// {
//     if (vx_can_set == NULL || vy_can_set == NULL || wz_can_set == NULL || )
//     {
//         return;
//     }
//     *vx_can_set = 0.0f;
//     *vy_can_set = 0.0f;
//     *wz_can_set = 0.0f;
// }

/**
  * @brief          底盘不移动的行为状态机下，底盘模式是不跟随角度，
  * @author         RM
  * @param[in]      vx_set前进的速度,正值 前进速度， 负值 后退速度
  * @param[in]      vy_set左右的速度,正值 左移速度， 负值 右移速度
  * @param[in]      wz_set旋转的速度，旋转速度是控制底盘的底盘角速度
  * @param[in]      chassis_move_rc_to_vector底盘数据
  * @retval         返回空
  */

// static void chassis_no_move_control(fp32 *vx_set, fp32 *vy_set, fp32 *wz_set, chassis_move_t *chassis_move_rc_to_vector)
// {
//     if (vx_set == NULL || vy_set == NULL || wz_set == NULL || )
//     {
//         return;
//     }
//     *vx_set = 0.0f;
//     *vy_set = 0.0f;
//     *wz_set = 0.0f;
// }

/**
  * @brief          底盘跟随云台的行为状态机下，底盘模式是跟随云台角度，底盘旋转速度会根据角度差计算底盘旋转的角速度
  * @author         RM
  * @param[in]      vx_set前进的速度,正值 前进速度， 负值 后退速度
  * @param[in]      vy_set左右的速度,正值 左移速度， 负值 右移速度
  * @param[in]      angle_set底盘与云台控制到的相对角度
  * @param[in]      chassis_move_rc_to_vector底盘数据
  * @retval         返回空
  */

// static void chassis_infantry_follow_gimbal_yaw_control(fp32 *vx_set, fp32 *vy_set, fp32 *angle_set, chassis_move_t *chassis_move_rc_to_vector)
// {
//     if (vx_set == NULL || vy_set == NULL || angle_set == NULL || )
//     {
//         return;
//     }

//     //遥控器的通道值以及键盘按键 得出 一般情况下的速度设定值
//     chassis_rc_to_control_vector(vx_set, vy_set, chassis_move_rc_to_vector);

//     //摇摆角度是利用sin函数生成，swing_time 是sin函数的输入值
//     static fp32 swing_time = 0.0f;
    
//     static fp32 swing_angle = 0.0f;
//     //max_angle 是sin函数的幅值
//     static fp32 max_angle = SWING_NO_MOVE_ANGLE;
//     //swing_time 在一个控制周期内，加上 add_time
//     static fp32 const add_time = PI * 0.5f * configTICK_RATE_HZ / CHASSIS_CONTROL_TIME_MS;
    
//     static uint8_t swing_flag = 0;

//     //判断是否要摇摆
//     if (chassis_move_rc_to_vector->chassis_RC->key.v & SWING_KEY)
//     {
//         if (swing_flag == 0)
//         {
//             swing_flag = 1;
//             swing_time = 0.0f;
//         }
//     }
//     else
//     {
//         swing_flag = 0;
//     }

//     //判断键盘输入是不是在控制底盘运动，底盘在运动减小摇摆角度
//     if (chassis_move_rc_to_vector->chassis_RC->key.v & CHASSIS_FRONT_KEY || chassis_move_rc_to_vector->chassis_RC->key.v & CHASSIS_BACK_KEY ||
//         chassis_move_rc_to_vector->chassis_RC->key.v & CHASSIS_LEFT_KEY || chassis_move_rc_to_vector->chassis_RC->key.v & CHASSIS_RIGHT_KEY)
//     {
//         max_angle = SWING_MOVE_ANGLE;
//     }
//     else
//     {
//         max_angle = SWING_NO_MOVE_ANGLE;
//     }
    
//     if (swing_flag)
//     {
//         swing_angle = max_angle * arm_sin_f32(swing_time);
//         swing_time += add_time;
//     }
//     else
//     {
//         swing_angle = 0.0f;
//     }
//     //sin函数不超过2pi
//     if (swing_time > 2 * PI)
//     {
//         swing_time -= 2 * PI;
//     }


//     *angle_set = swing_angle;
// }

/**
  * @brief          底盘跟随底盘yaw的行为状态机下，底盘模式是跟随底盘角度，底盘旋转速度会根据角度差计算底盘旋转的角速度
  * @author         RM
  * @param[in]      vx_set前进的速度,正值 前进速度， 负值 后退速度
  * @param[in]      vy_set左右的速度,正值 左移速度， 负值 右移速度
  * @param[in]      angle_set底盘设置的yaw，范围 -PI到PI
  * @param[in]      chassis_move_rc_to_vector底盘数据
  * @retval         返回空
  */

// static void chassis_engineer_follow_chassis_yaw_control(fp32 *vx_set, fp32 *vy_set, fp32 *angle_set, chassis_move_t *chassis_move_rc_to_vector)
// {
//     if (vx_set == NULL || vy_set == NULL || angle_set == NULL || )
//     {
//         return;
//     }

//     chassis_rc_to_control_vector(vx_set, vy_set, chassis_move_rc_to_vector);

//     *angle_set = rad_format(chassis_move_rc_to_vector->chassis_yaw_set - CHASSIS_ANGLE_Z_RC_SEN * chassis_move_rc_to_vector->chassis_RC->rc.ch[CHASSIS_WZ_CHANNEL]);
// }

/**
  * @brief          底盘不跟随角度的行为状态机下，底盘模式是不跟随角度，底盘旋转速度由参数直接设定
  * @author         RM
  * @param[in]      vx_set前进的速度,正值 前进速度， 负值 后退速度
  * @param[in]      vy_set左右的速度,正值 左移速度， 负值 右移速度
  * @param[in]      wz_set底盘设置的旋转速度,正值 逆时针旋转，负值 顺时针旋转
  * @param[in]      chassis_move_rc_to_vector底盘数据
  * @retval         返回空
  */

// static void chassis_no_follow_yaw_control(fp32 *vx_set, fp32 *vy_set, fp32 *wz_set, chassis_move_t *chassis_move_rc_to_vector)
// {
//     if (vx_set == NULL || vy_set == NULL || wz_set == NULL || )
//     {
//         return;
//     }

//     chassis_rc_to_control_vector(vx_set, vy_set, chassis_move_rc_to_vector);
//     *wz_set = -CHASSIS_WZ_RC_SEN * chassis_move_rc_to_vector->chassis_RC->rc.ch[CHASSIS_WZ_CHANNEL];
// }

/**
  * @brief          底盘开环的行为状态机下，底盘模式是raw原生状态，故而设定值会直接发送到can总线上
  * @param[in]      vx_set前进的速度,正值 前进速度， 负值 后退速度
  * @param[in]      vy_set左右的速度，正值 左移速度， 负值 右移速度
  * @param[in]      wz_set 旋转速度， 正值 逆时针旋转，负值 顺时针旋转
  * @param[in]      chassis_move_rc_to_vector底盘数据
  * @retval         none
  */

// static void chassis_open_set_control(fp32 *vx_set, fp32 *vy_set, fp32 *wz_set, chassis_move_t *chassis_move_rc_to_vector)
// {
//     if (vx_set == NULL || vy_set == NULL || wz_set == NULL || )
//     {
//         return;
//     }

//     *vx_set = chassis_move_rc_to_vector->chassis_RC->rc.ch[CHASSIS_X_CHANNEL] * CHASSIS_OPEN_RC_SCALE;
//     *vy_set = -chassis_move_rc_to_vector->chassis_RC->rc.ch[CHASSIS_Y_CHANNEL] * CHASSIS_OPEN_RC_SCALE;
//     *wz_set = -chassis_move_rc_to_vector->chassis_RC->rc.ch[CHASSIS_WZ_CHANNEL] * CHASSIS_OPEN_RC_SCALE;
//     return;
// }
